﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace fhe
{
  using math;
  using core;
  namespace physics
  {
    public class Body : Node, IUpdatable, ICollidable
    {
      public List<PointMass> PointMasses { get; set; }

      public List<Spring> Springs { get; set; }

      public List<Face> Faces { get; set; }

      public double PropGain { get; set; }

      public double DiffGain { get; set; }

      public double Friction { get; set; }

      public const double DefaultPropGain = 100;

      public const double DefaultDiffGain = 1;

      public const double DefaultFriction = 0.1;

      public Body()
      {
        PointMasses = new List<PointMass>();
        Springs = new List<Spring>();
        Faces = new List<Face>();
        PropGain = DefaultPropGain;
        DiffGain = DefaultDiffGain;
        Friction = DefaultFriction;
      }

      private List<Vec2> GetVertices()
      {
        return PointMasses.Select(pm => pm.Position).ToList();
      }

      public Circle GetBoundingCircle()
      {
        return ShapeUtils.GetBoundingCircle(GetVertices());
      }

      public AABB GetBoundingBox()
      {
        return ShapeUtils.GetBoundingBox(GetVertices());
      }

      private bool EdgesEqual(PolygonUtils.Pair<int> e1, PolygonUtils.Pair<int> e2)
      {
        return (e1.A == e2.A && e1.B == e2.B) || (e1.A == e2.B && e1.B == e2.A);
      }

      private bool EdgeIsUnique(int i, List<PolygonUtils.Pair<int>> edges)
      {
        return !Enumerable.Range(0, edges.Count).Any(j => i != j && EdgesEqual(edges[i], edges[j]));
      }

      private List<int> GetPerimeter()
      {
        List<PolygonUtils.Pair<int>> edges = Faces
          .SelectMany(face => face.Indices.Wrap())
          .ToList();
        List<PolygonUtils.Pair<int>> uniqueEdges = Enumerable
          .Range(0, edges.Count)
          .Where(i => EdgeIsUnique(i, edges))
          .Select(i => edges[i])
          .ToList();
        int last = uniqueEdges[0].A;
        List<int> perimeter = new List<int>() { last };
        while (uniqueEdges.Any())
        {
          List<PolygonUtils.Pair<int>> removeEdges = new List<PolygonUtils.Pair<int>>();
          foreach (PolygonUtils.Pair<int> edge in uniqueEdges)
          {
            if (edge.A == last)
            {
              last = edge.B;
              perimeter.Add(last);
              removeEdges.Add(edge);
            }
            else if (edge.B == last)
            {
              last = edge.A;
              perimeter.Add(last);
              removeEdges.Add(edge);
            }
          }
          if (!removeEdges.Any())
          {
            throw new Exception();
          }
          removeEdges.ForEach(e => uniqueEdges.Remove(e));
        }
        if (perimeter.First() != perimeter.Last())
        {
          throw new Exception();
        }
        return perimeter.Skip(1).ToList();
      }

      private List<Polygon> GetFacePolygons()
      {
        return Faces.Select(face => new Polygon() { Vertices = face.Indices.Select(i => PointMasses[i].Position).ToList() }).ToList();
      }

      public bool Inside(Vec2 v)
      {
        return GetBoundingBox().Inside(v) && GetFacePolygons().Any(poly => poly.Inside(v));
      }

      public Polygon GetBoundingPolygon()
      {
        return new Polygon() { Indices = GetPerimeter(), GetVertex = i => PointMasses[i].Position };
      }

      public void OffsetTo(Vec2 v, Vec2? dir, out Vec2 offset, out List<PointMass> points)
      {
        List<int> indices;
        GetBoundingPolygon().SurfaceOffset(v, dir, out offset, out indices);
        points = indices.Select(i => PointMasses[i]).ToList();
      }

      public void Collide(ICollidable body)
      {
        Vec2 bodyOffset = GetBoundingCircle().Center - body.GetBoundingCircle().Center;
        foreach (PointMass pointMass in PointMasses)
        {
          if (body.Inside(pointMass.Position))
          {
            Vec2 offset;
            List<PointMass> points;
            body.OffsetTo(pointMass.Position, bodyOffset, out offset, out points);

            Vec2 surfaceVel = points
              .Select(point => point.Velocity)
              .Aggregate((v1, v2) => v1 + v2)
              / (double)points.Count;
            Vec2 relVel = pointMass.Velocity - surfaceVel;
            Vec2 projVel = relVel.Project(offset);

            Vec2 offsetForce = offset * PropGain - projVel * DiffGain;

            Vec2 perpOffset = offset.Perp();
            Vec2 perpVel = relVel.Project(perpOffset);
            Vec2 frictionForce = perpVel * -body.Friction;

            Vec2 force = offsetForce + frictionForce;
            pointMass.ApplyForce(force);
            points.ForEach(point => point.ApplyForce(force / (double)points.Count));
          }
        }
      }

      public double Mass { get { return PointMasses.Sum(pm => pm.Mass); } }

      public void Update(TimeSpan ts)
      {
        Springs.ForEach(s => s.Update(ts));
        PointMasses.ForEach(pm => pm.Update(ts));
        if (Mass > 0)
        {
          //Console.WriteLine(GetBoundingCircle().Center);
          Console.WriteLine(string.Join(", ", PointMasses.Select(pm => pm.Position)));
        }
      }

      public void FullyConnectSprings(double propGain = Spring.DefaultPropGain, double diffGain = Spring.DefaultDiffGain)
      {
        for (int i = 0; i < PointMasses.Count; ++i)
        {
          for (int j = i + 1; j < PointMasses.Count; ++j)
          {
            Springs.Add(new Spring()
            {
              P1 = PointMasses[i],
              P2 = PointMasses[j],
              PropGain = propGain,
              DiffGain = diffGain,
            });
          }
        }
      }

      public static Body Rectangle(Vec2 min,
        Vec2 max,
        double mass = 1,
        double propGain = DefaultPropGain,
        double diffGain = DefaultDiffGain,
        double friction = DefaultFriction)
      {
        Body body = new Body()
        {
          PointMasses = new List<Vec2>() { new Vec2(0, 0), new Vec2(0, 1), new Vec2(1, 1), new Vec2(1, 0) }
            .Select(d => new PointMass() { Position = min + (max - min) * d, Mass = mass / 4 })
            .ToList(),
          Faces = new List<Face>(){
            new Face(){Indices = new List<int>(){0,1,2}},
            new Face(){Indices = new List<int>(){0,2,3}},
          },
          PropGain = propGain,
          DiffGain = diffGain,
          Friction = friction,
        };
        body.FullyConnectSprings();
        return body;
      }
    }
  }
}
